import open3d as o3d
import numpy as np
import cv2
import os

'''用于将点云投影到 xoy 平面上，并保存为图像'''


# 获取点云的坐标与标签颜色    传入筛选高度的阈值
def get_2d_point(cloud_path, height_threshold=1.0):
    pcd = o3d.io.read_point_cloud(cloud_path)
    points = np.asarray(pcd.points)
    colors = np.asarray(pcd.colors)

    max_z_points = {}  # 存储最大高度的点以及颜色
    max_z_colors = {}

    for i in range(points.shape[0]):
        x, y, z = points[i]
        if z > height_threshold:
            continue  # 如果点的高度大于阈值，跳过该点

        key = (int(x * 10), int(y * 10))  # 使用整数作为键，乘以10是为了提高分辨率
        if key not in max_z_points or z > max_z_points[key]:
            max_z_points[key] = z
            max_z_colors[key] = colors[i]
    projected_points = np.array(list(max_z_points.keys()))
    projected_colors = np.array(list(max_z_colors.values()))

    return projected_points, projected_colors


# 将可行域进行替换
def replace_colors(points, colors, target_color_list, new_color_list):
    # 将颜色值转换为0-1之间的值
    target_colors = np.array(target_color_list) / 255.0
    new_colors = np.array(new_color_list) / 255.0

    for target_color, new_color in zip(target_colors, new_colors):
        # 找到所有颜色匹配的点
        mask = np.all(colors == target_color, axis=1)
        # 替换颜色
        colors[mask] = new_color

    return points, colors


# 绘制栅格地图+转换灰度图
def plot_2d_map(projected_points, projected_colors, color_image_path, grey_image_path):
    min_bounds = np.min(projected_points, axis=0)
    max_bounds = np.max(projected_points, axis=0)
    image_size = (max_bounds - min_bounds + 1).astype(int)

    # image = np.ones((image_size[1], image_size[0], 3), dtype=np.uint8) * 255
    # 初始化一个灰色的背景图
    image = np.ones((image_size[0], image_size[1], 3), dtype=np.uint8) * np.array([128, 128, 128], dtype=np.uint8)
    # image = np.ones((image_size[0], image_size[1], 3), dtype=np.uint8) * 255

    for i in range(projected_points.shape[0]):
        x, y = projected_points[i] - min_bounds
        x, y = int(x), int(y)
        # image[y, x] = (projected_colors[i] * 255).astype(np.uint8)
        image[x, y] = (projected_colors[i] * 255).astype(np.uint8)

    # 应用中值滤波去噪
    # denoised_image = cv2.medianBlur(image, 3)  # 使用3x3的窗口
    # 保存彩色图像
    cv2.imwrite(color_image_path, image)
    print(f"Color image saved as {color_image_path}")

    # 转换为灰度图并保存
    grey_image = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
    cv2.imwrite(grey_image_path, grey_image)
    print(f"Grey image saved as {grey_image_path}")

    # # 计算灰度图中所有唯一的灰度值
    # unique_gray_values = np.unique(grey_image)
    #
    # # 打印唯一灰度值的数量和具体值
    # num_unique_gray_values = len(unique_gray_values)
    # print(f"Number of unique gray values in the grey image: {num_unique_gray_values}")
    # print(f"Unique gray values: {unique_gray_values}")


if __name__ == "__main__":
    cloud_path = 'data/save_cloud1.pcd'
    grid_map_path = 'data'
    color_name = 'color_0905_01.png'
    grey_name = 'grey_0905_01.pgm'
    color_image_path = os.path.join(grid_map_path, color_name)
    grey_image_path = os.path.join(grid_map_path, grey_name)
    # 把道路与人行道置换成白色；未建图地区置换成灰色
    target_color_list = [[128, 64, 128], [0, 0, 192]]
    new_color_list = [[255, 255, 255], [255, 255, 255]]
    projected_points, projected_colors = get_2d_point(cloud_path)
    new_points, new_colors = replace_colors(projected_points, projected_colors, target_color_list, new_color_list)
    plot_2d_map(new_points, new_colors, color_image_path, grey_image_path)

# label_info = {
#     1: (128, 0, 0),  # building - Red
#     2: (128, 128, 128),  # sky - Grey
#     3: (128, 64, 128),  # road - Pink  偏紫色
#     4: (128, 128, 0),  # vegetation - Dark yellow
#     5: (0, 0, 192),  # sidewalk/pave - Blue
#     6: (64, 0, 128),  # car - Purple  蓝紫色
#     7: (64, 64, 0),  # pedestrian - Yellow-brown
#     8: (0, 128, 192),  # cyclist - Light blue
#     9: (192, 128, 128),  # signage - Salmon
#     10: (64, 64, 128),  # fence/wall - Grey-purple
#     11: (192, 192, 128),  # pole - Light yellow
#     0: (0, 0, 0)  # other - Black
# }
# https://www.qianbo.com.cn/Tool/Rgba/      查看颜色对照
